#include "MOTOR.h"
void motor_delay_nms(u16 time)
{
    u16 i=0;
    while(time--)
    {
        i=10000;
        while(i--) ;
    }
}
void Motor_Init()
{
    TIM_TimeBaseInitTypeDef timdef;
    TIM_OCInitTypeDef timocdef;
    GPIO_InitTypeDef iodef;
    
#ifdef USING_BOARD_YELLOW
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    //  TIME4 
    //  CH1 B6,  一号
    //  CH2 B7,  二号
    //  CH3 B8,  三号
    //  CH4 B9   四号
    iodef.GPIO_Mode=GPIO_Mode_AF_PP;
    iodef.GPIO_Pin=GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;
    iodef.GPIO_Speed=GPIO_Speed_50MHz;
    
    GPIO_Init(GPIOB,&iodef);
    timdef.TIM_Period=1999; //计数长度2000  
    timdef.TIM_Prescaler=63;//计算频率1m    500hz
    timdef.TIM_ClockDivision=TIM_CKD_DIV1;
    timdef.TIM_CounterMode=TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM4,&timdef);
    
    if(fly_mode==Correct_ESC)
    { 
        timocdef.TIM_Pulse =1950;
    }
    else
    {
        timocdef.TIM_Pulse =1000;
    }
    TIM_OCStructInit(&timocdef);
    
    timocdef.TIM_OCMode = TIM_OCMode_PWM1;  //选择定时器模式:TIM脉冲宽度调制模式1
    timocdef.TIM_OutputState = TIM_OutputState_Enable;  //比较输出使能
    timocdef.TIM_OCPolarity = TIM_OCPolarity_High;     //输出极性:TIM输出比较极性高
   
    TIM_OC1Init(TIM4,&timocdef);
    TIM_OC1PreloadConfig(TIM4,TIM_OCPreload_Enable); //使能TIM4在CCR1上的预装载寄存器
    
    TIM_OC2Init(TIM4,&timocdef);
    TIM_OC2PreloadConfig(TIM4,TIM_OCPreload_Enable);
    
    TIM_OC3Init(TIM4,&timocdef);
    TIM_OC3PreloadConfig(TIM4,TIM_OCPreload_Enable);
    
    TIM_OC4Init(TIM4,&timocdef);
    TIM_OC4PreloadConfig(TIM4,TIM_OCPreload_Enable);
    
    TIM_Cmd(TIM4, ENABLE);
    TIM_CtrlPWMOutputs(TIM4, ENABLE);
//    if(fly_mode==Correct_ESC)
//    {
//        motor_delay_nms(2500);
//     
//    }  
//    Motor_Duty(1000,1000,1000,1000);
#endif  
#ifdef USING_BOARD_RED
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_AFIO, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    //  TIME4 
    //  CH1 B6,  一号
    //  CH2 B7,  二号
    //  CH3 B8,  三号
    //  CH4 B9   四号
    iodef.GPIO_Mode=GPIO_Mode_AF_PP;
    iodef.GPIO_Pin=GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;
    iodef.GPIO_Speed=GPIO_Speed_50MHz;
    
    GPIO_Init(GPIOB,&iodef);
    timdef.TIM_Period=1999; //计数长度2000  
    timdef.TIM_Prescaler=63;//计算频率1m    500hz
    timdef.TIM_ClockDivision=TIM_CKD_DIV1;
    timdef.TIM_CounterMode=TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM4,&timdef);
    
    if(fly_mode==Correct_ESC)
    { 
        timocdef.TIM_Pulse =1950;
    }
    else
    {
        timocdef.TIM_Pulse =1000;
    }  
    TIM_OCStructInit(&timocdef);
    timocdef.TIM_OCMode = TIM_OCMode_PWM1;  //选择定时器模式:TIM脉冲宽度调制模式1
    timocdef.TIM_OutputState = TIM_OutputState_Enable;  //比较输出使能
    timocdef.TIM_OCPolarity = TIM_OCPolarity_High;     //输出极性:TIM输出比较极性高
  
    
    TIM_OC1Init(TIM4,&timocdef);
    TIM_OC1PreloadConfig(TIM4,TIM_OCPreload_Enable); //使能TIM4在CCR1上的预装载寄存器
    
    TIM_OC2Init(TIM4,&timocdef);
    TIM_OC2PreloadConfig(TIM4,TIM_OCPreload_Enable);
    
    TIM_OC3Init(TIM4,&timocdef);
    TIM_OC3PreloadConfig(TIM4,TIM_OCPreload_Enable);
    
    TIM_OC4Init(TIM4,&timocdef);
    TIM_OC4PreloadConfig(TIM4,TIM_OCPreload_Enable);
    
    TIM_Cmd(TIM4, ENABLE);
    TIM_CtrlPWMOutputs(TIM4, ENABLE);
//    if(fly_mode==Correct_ESC)
//    {
//        motor_delay_nms(2500);
//        Motor_Duty(1000,1000,1000,1000);
//    }
#endif  
}
void motor_correct()
{
   __set_PRIMASK(0);	// 开中断
  if(ReadButton()&BTN1_PRESS) 
    fly_mode=Correct_ESC;
  else      
    fly_mode=Waitting;
  while(fly_mode==Correct_ESC)
  {
        convert_ppm();
        Motor_Duty(rc_target.Throttle,rc_target.Throttle,rc_target.Throttle,rc_target.Throttle);
        if(!(ReadButton()&BTN1_PRESS)) break; 
  }
  __set_PRIMASK(1);	
}
void Motor_Duty(uint16_t m1,uint16_t m2,uint16_t m3,uint16_t m4)
{
    //TIME3 CH1 A6, CH2 A7,TIM4 CH3 B0,CH4 B1
    //一号 B1 二号 B0 三号 A7 四号 A6
//    m1=m1>MOTOR_PWM_MAX?1951:m1;
//    m2=m2>MOTOR_PWM_MAX?1951:m2;
//    m3=m3>MOTOR_PWM_MAX?1951:m3;
//    m4=m4>MOTOR_PWM_MAX?1951:m4;
    TIM4->CCR1=m4-1;
    TIM4->CCR2=m3-1;
    TIM4->CCR3=m2-1;
    TIM4->CCR4=m1-1;
//        TIM4->CCR1=m1-1;
//    TIM4->CCR2=m2-1;
//    TIM4->CCR3=m3-1;
//    TIM4->CCR4=m4-1;
}

